#!/usr/bin/env python
# -*- encoding: utf-8 -*-
'''
@File         :formation_controller.py
@Description  :编队控制器的父类
@Time         :2024/05/23 16:01:15
@Author       :Lin Yuheng
@Version      :1.0
'''

import rospy
from tools.formation_structure import *
from tools.mathlib import *
from tools.vector import *
from tools.filter import *
from tools.increment_pid_controller import *

class FormationController:
    def __init__(self):

        # 控制时间间隔
        self._dt = 0.02
        # 控制时间间隔max
        self._dt_max = 0.1
        # 控制时间间隔min
        self._dt_min = 0.01
        
        # 编队队形偏移量
        self.formation_offset = S_FwFormationOffset()
        # 飞机模型参数
        self.fw_params = S_FwModelParams()

        # 领机状态(虚拟领机)
        self.leader_states = S_FwLeaderStates()
        # 本机状态
        self.fw_state = S_FwStates()

        # 领机机正在飞行标志位
        self.led_in_fly = False
        # 本机正在飞行标志位
        self.fol_in_fly = False

        # 最后4通道控制量
        self.fw_cmd = S_FW4Cmd()
        # 本机期望设定点
        self.fw_sp = S_FwSetPoint()
        # 本机误差，包括与与领机的偏差
        self.fw_error = S_FwError()

        

    def update_led_fol_states(self, leader_states_:S_FwLeaderStates, thisfw_state:S_FwStates):
        """
        @Description : 更新领机，本机飞行状态
        @Param:
        @Returns     :
        """
        self.leader_states = leader_states_
        self.fw_state = thisfw_state

    def set_formation_type(self, formation_type:int):
        """
        @Description : 设定编队队形
        @Param:
        @Returns     :
        """
        # TODO 设定不同的编队期望队形

    def set_fw_model_params(self, input_params:S_FwModelParams):
        """
        @Description : 设定编队控制器内部中飞机模型参数
        @Param:
        @Returns     :
        """
        self.fw_params = input_params

    
    def control_formation(self):
        """
        编队控制器主函数(子类实现)
        """
        pass

    def get_formation_4cmd(self):
        """
        @Description : 获得飞机期望的4通道控制量
        @Param:
        @Returns     :
        """
        return self.fw_cmd
    
    def get_formation_sp(self):
        """
        @Description : 获得编队中本机的运动学期望值
        @Param:
        @Returns     :
        """
        return self.fw_sp
    
    def get_formation_error(self):
        """
        @Description : 获得编队控制误差
        @Param:
        @Returns     :
        """
        return self.fw_error
    
    def identify_led_fol_states(self):
        """
        @Description : 判断领机，本机是否在飞行
        @Param:
        @Returns     :
        """
        if abs_num (self.leader_states.global_vel_n) > 3.0 or abs_num (self.leader_states.global_vel_e) > 3.0 or self.leader_states.relative_alt > 3.0:
            self.led_in_fly = True
        else:
            
            self.led_in_fly = False
            rospy.loginfo("领机未在飞行之中")

        if abs_num (self.fw_state.global_vel_n > 3.0) or abs_num (self.fw_state.global_vel_e > 3.0) or self.fw_state.relative_alt > 3.0:
            self.fol_in_fly = True
        else:
            self.fol_in_fly = False
            rospy.loginfo("本机未在飞行之中")
        
        if self.fol_in_fly and self.led_in_fly:
            return True
        else:
            return False